package main;

import java.util.Iterator;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.RegulatedMotor;
import controller.Instruction;
import controller.Map;
import controller.MapPoint;
import controller.SampleMap;
import controller.Scenario;

public class OdometryTest extends Scenario {

	static RegulatedMotor leftMotor = Motor.A;
	static RegulatedMotor rightMotor = Motor.C;

	public OdometryTest(Map map) {
		super(map);
	}

	@Override
	public void execute() throws InterruptedException {

		leftMotor.setSpeed(50);
		rightMotor.setSpeed(50);

		LCD.clear();
		LCD.drawString("Clique para iniciar", 3, 4);
		LCD.refresh();

		while (!Button.ESCAPE.isDown()) {

		}

		LCD.clear();
		LCD.drawString("Teste odometria iniciado", 3, 4);
		LCD.refresh();

		Iterator<MapPoint> pointsIterator = map.getPoints().iterator();

		// o ponto inicial deve ser ignorado
		//pointsIterator.next();

		while (pointsIterator.hasNext()) {
			MapPoint point = pointsIterator.next();
			int distance = point.getDistance();
			for (int i = 0; i < distance; i++) {
				leftMotor.rotate(-10, true);
				rightMotor.rotate(-10, true);
				
				LCD.clear();
				LCD.drawString(String.valueOf(i), 3, 4);
				LCD.refresh();
				Thread.sleep(200);

			}

			LCD.clear();
			LCD.drawString("Ponto " + point.getId(), 3, 4);
			LCD.refresh();
			turn(point);
			Thread.sleep(5000);
		}
	}

	private void turn(MapPoint point) throws InterruptedException {
		if (point.getInstruction() == Instruction.TURN_LEFT) {
			leftMotor.rotate(-400, true);
			rightMotor.rotate(400, true);
			Thread.sleep(2000);
			leftMotor.rotate(-252, true);
			rightMotor.rotate(252, true);
			Thread.sleep(2000);
		}
		if (point.getInstruction() == Instruction.TURN_RIGHT) {
			leftMotor.rotate(400, true);
			rightMotor.rotate(-400, true);
			Thread.sleep(2000);
			leftMotor.rotate(252, true);
			rightMotor.rotate(-252, true);
			Thread.sleep(2000);
			
		}
	}

	public static void main(String[] args) {
		try {
			new OdometryTest(new SampleMap()).execute();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

}
